#include <ros/ros.h>
#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/GetConstraintAwarePositionIK.h>
#include <arm_navigation_msgs/SetPlanningSceneDiff.h>

static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";

int main(int argc, char ** argv) {
   ros::init(argc, argv, "get_ik");
   ros::NodeHandle rh;
   
   ros::service::waitForService("pr2_right_arm_kinematics/get_ik_solver_info");
   ros::service::waitForService("pr2_right_arm_kinematics/get_constraint_aware_ik");
   ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
   
   ros::ServiceClient planning_scene_client = rh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff> (SET_PLANNING_SCENE_DIFF_NAME);
   ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo> ("pr2_right_arm_kinematics/get_ik_solver_info");
   ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK> ("pr2_right_arm_kinematics/get_constraint_aware_ik");

   //Set planning scene with current joint state??
   arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
   arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
   planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.stamp = ros::Time::now();
   planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names.push_back("world_joint");
   planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids.push_back("odom_combined");
   planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids.push_back("base_footprint)");
   planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.poses.resize(1);
   planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.poses[0].position.x = 4.0; //?
   planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.poses[0].orientation.w = 1.0;
   
   if(!planning_scene_client.call(planning_scene_req, planning_scene_res)) {
      ROS_WARN("Can't set planning scene.");
      return -1;
   }

   kinematics_msgs::GetKinematicSolverInfo::Request request;
   kinematics_msgs::GetKinematicSolverInfo::Response response;

   if(query_client.call(request,response))
      for(unsigned int i=0; i<response.kinematic_solver_info.joint_names.size(); i++)
         ROS_DEBUG("Joint: %d %s", i, response.kinematic_solver_info.joint_names[i].c_str());
   else {
      ROS_ERROR("Could not call query service");
      ros::shutdown();
      exit(-1);
   }

   kinematics_msgs::GetConstraintAwarePositionIK::Request gpik_req;
   kinematics_msgs::GetConstraintAwarePositionIK::Response gpik_res;

   gpik_req.timeout = ros::Duration(5.0);
   gpik_req.ik_request.ik_link_name = "r_wrist_roll_link";
   gpik_req.ik_request.pose_stamped.header.frame_id = "torso_lift_link";
   gpik_req.ik_request.pose_stamped.pose.position.x = 0.75;
   gpik_req.ik_request.pose_stamped.pose.position.y = -0.188;
   gpik_req.ik_request.pose_stamped.pose.position.z = 0.0;
   gpik_req.ik_request.pose_stamped.pose.orientation.x = 0.0;
   gpik_req.ik_request.pose_stamped.pose.orientation.y = 0.0;
   gpik_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
   gpik_req.ik_request.pose_stamped.pose.orientation.w = 1.0;

   gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
   gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;

   for (unsigned int i=0; i<response.kinematic_solver_info.joint_names.size(); i++) 
      gpik_req.ik_request.ik_seed_state.joint_state.position[i] = (response.kinematic_solver_info.limits[i].min_position + response.kinematic_solver_info.limits[i].max_position)/2.0;

   if (ik_client.call(gpik_req, gpik_res))
      if(gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
         for(unsigned int i = 0; i < gpik_res.solution.joint_state.name.size(); i++)
            ROS_INFO("Joint %s %f", gpik_res.solution.joint_state.name[i].c_str(), gpik_res.solution.joint_state.position[i]);
      else
         ROS_ERROR("Inverse kinematics failed.");
   else
      ROS_ERROR("Inverse kinematics service call failed");
   
   ros::shutdown();

}
